#include "rviz_custom_plugins/panels/command_panel.h"
#include <chrono>


using namespace std::chrono_literals;


namespace rviz_custom_plugins {


namespace panels {


CommandPanel::CommandPanel(QWidget *parent) : rviz_common::Panel(parent), ui(new Ui::commandPanel())
{
    ui->setupUi(this);
    widgetManager = WidgetManager::getInstance();
}


void CommandPanel::load(const Config& config)
{
    Panel::load(config);
}


void CommandPanel::onInitialize()
{
    node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
    oprCmdClient = node->create_client<ControlCmd>("visbot_opr_cmd");

    connect(ui->btnStart, &QPushButton::clicked, this, &CommandPanel::startBtnClicked);
    connect(ui->btnStop, &QPushButton::clicked, this, &CommandPanel::stopBtnClicked);
    connect(ui->btnPause, &QPushButton::clicked, this, &CommandPanel::pauseBtnClicked);
    connect(ui->btnResume, &QPushButton::clicked, this, &CommandPanel::resumeBtnClicked);
    connect(ui->btnSwitch, &QPushButton::clicked, this, &CommandPanel::switchBtnClicked);
    connect(ui->btnActive, &QPushButton::clicked, this, &CommandPanel::activeBtnClicked);
    connect(ui->btnInActive, &QPushButton::clicked, this, &CommandPanel::inActiveBtnClicked);
    connect(ui->btnReturn, &QPushButton::clicked, this, &CommandPanel::returnBtnClicked);
    connect(ui->btnLand, &QPushButton::clicked, this, &CommandPanel::landBtnClicked);
    connect(ui->btnPhoto, &QPushButton::clicked, this, &CommandPanel::photoBtnClicked);
    connect(ui->btnVideo, &QPushButton::clicked, this, &CommandPanel::videoBtnClicked);

    connect(widgetManager, &WidgetManager::widgetShow, this, &CommandPanel::handleWidgetShow);
    setEnabled(false);
    parentWidget()->hide();
}


void CommandPanel::sendControlCmd(std::shared_ptr<ControlCmd::Request> request)
{
    bool isServiceValid = oprCmdClient->wait_for_service(0.5s);
    if(!isServiceValid) {
        RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "opr service not available...");
        return;
    }

    oprCmdClient->async_send_request(
        request,
        [this](rclcpp::Client<ControlCmd>::SharedFuture future)
        {
            auto response = future.get();
            if(response->result != 512) 
            {
                RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "snapshot failed");
                return;
            }
        }
    );   
}


void CommandPanel::startBtnClicked()
{
    auto request = std::make_shared<ControlCmd::Request>();
    request->opr_cmd = 1;
    request->drone_id = 0;
    sendControlCmd(request);
}


void CommandPanel::stopBtnClicked()
{
    auto request = std::make_shared<ControlCmd::Request>();
    request->opr_cmd = 2;
    request->drone_id = 0;
    sendControlCmd(request);
}


void CommandPanel::pauseBtnClicked()
{
    auto request = std::make_shared<ControlCmd::Request>();
    request->opr_cmd = 3;
    request->drone_id = 0;
    sendControlCmd(request);
}


void CommandPanel::resumeBtnClicked()
{
    auto request = std::make_shared<ControlCmd::Request>();
    request->opr_cmd = 4;
    request->drone_id = 0;
    sendControlCmd(request);
}


void CommandPanel::switchBtnClicked()
{
    auto request = std::make_shared<ControlCmd::Request>();
    request->opr_cmd = 5;
    request->drone_id = 0;
    sendControlCmd(request);
}


void CommandPanel::activeBtnClicked()
{
    auto request = std::make_shared<ControlCmd::Request>();
    request->opr_cmd = 6;
    request->drone_id = 0;
    sendControlCmd(request);
}


void CommandPanel::inActiveBtnClicked()
{
    auto request = std::make_shared<ControlCmd::Request>();
    request->opr_cmd = 7;
    request->drone_id = 0;
    sendControlCmd(request);
}



void CommandPanel::returnBtnClicked()
{
    auto request = std::make_shared<ControlCmd::Request>();
    request->opr_cmd = 8;
    request->drone_id = 0;
    sendControlCmd(request);
}


void CommandPanel::landBtnClicked()
{
    auto request = std::make_shared<ControlCmd::Request>();
    request->opr_cmd = 9;
    request->drone_id = 0;
    sendControlCmd(request);
}


void CommandPanel::photoBtnClicked()
{
    auto request = std::make_shared<ControlCmd::Request>();
    request->opr_cmd = 20;
    request->drone_id = 0;
    request->param_int = -1;
    sendControlCmd(request);
}


void CommandPanel::videoBtnClicked()
{
    auto request = std::make_shared<ControlCmd::Request>();
    request->opr_cmd = 22;
    request->drone_id = 0;
    request->param_int = -1;
    sendControlCmd(request);
}


void CommandPanel::handleWidgetShow(int widgetId, bool isShow) {
    if(widgetId == WidgetManager::COMMAND_PANEL) {
        if (isShow) {
            setEnabled(true);
            parentWidget()->show();
        } else {
            setEnabled(false);
            parentWidget()->hide();
        }
    }
}


}


}



#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rviz_custom_plugins::panels::CommandPanel, rviz_common::Panel )